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Abstract 

A  mobile  manipulator  generally  possesses 
kinematic  redundancy,  which  requires  some 
method  for  selecting  the  configuration  appro¬ 
priate  to  the  task.  Evolutionary  Programming 
(EP)  has  been  used  successfully  to  select  an 
appropriate  configuration  using  a  large  penalty 
on  the  end-point  position  to  force  the  end-point 
to  the  goal  position  for  the  task.  This  paper 
demonstrates  that  using  an  equality  constraint 
for  the  end-point  position  results  in  much 
faster  convergence  to  approximately  the  same 
final  configuration.  The  reasons  for  this  in¬ 
crease  in  speed  are  examined  using  a  simplified 
model  to  determine  the  relative  size  of  their 
effects. 

Introduction 

The  use  of  mobile  manipulators  is  becoming 
more  prevalent  on  the  factory  floor,  undersea, 
and  in  space  applications.  Most  of  these  sys¬ 
tems  are  teleoperated  or  telepresence  systems 
[1],  relying  upon  the  operator  to  eliminate  any 
kinematic  redundancies  through  direct  isomor¬ 
phic  mappings.  To  make  these  systems  more 
autonomous,  the  kinematic  redundancies  must 
be  resolved  without  requiring  operator  interven¬ 
tion. 

Various  methods  have  been  proposed  to  search 
for  the  best  configuration  of  a  mobile  manipula¬ 
tor  for  a  given  task.  These  methods  include 
gradient  descent  searches  of  the  joint-space  of 
the  manipulator  used  by  Pin  and  Culioli  [2], 
and  proposed  by  Burdick  and  Seraji  [3],  simu¬ 


lated  annealing  used  by  Carriker  et  al 
[4],  and  EP,  an  evolutionary  search 
strategy,  used  by  McDonnell  et  al  [5]. 
The  EP  technique  was  found  to  be  an 
effective  search  technique  that  avoided 
local  optima. 

The  cost  function  for  optimization  used 
in  [5]  imposed  a  high  cost  on  deviations 
of  the  eno  point  position  from  the  goal 
position  to  force  the  end-point  to  be  at 
the  required  position  for  the  task.  Un¬ 
fortunately,  this  high  cost  relative  to  the 
remaining  elements  of  the  cost  function 
results  in  a  search  surface  that  is  difficult 
for  the  EP  algorithm  to  traverse  effi¬ 
ciently.  This  paper  proposes  constrain¬ 
ing  the  end-point  position  to  the  goal 
position  and  searching  the  remaining 
smaller-dimensional  space.  In  addition, 
this  paper  examines  the  effect  of  the 
ratio  of  weights  for  different  elements  of 
the  cost  function  on  the  efficiency  of  the 
EP  search  algorithm. 

Optimization  Criteria 

The  ultimate  configuration  of  the  manip¬ 
ulator  is  a  direct  function  of  the  choice 
of  the  optimization  criteria.  This  criteria 
must  take  into  account  the  end-point 
position,  obstacles  in  the  environment, 
static  torques  required  at  the  joints  due 
to  external  forces  and  the  weights  of  the 
links,  and  the  ability  of  the  manipulator 
to  perform  tasks  in  a  particular  config¬ 
uration,  known  as  the  manipulability. 


This  section  summarizes  the  criteria  developed 
in  [2]  and  [5]  to  determine  the  cost  associated 
with  mobile  manipulator  configurations. 

The  end-point  position  of  the  manipulator  is 
the  most  important  factor  in  the  cost  of  a 
configuration  since  the  manipulator  will  be 
unable  to  perform  the  task  if  it  is  not  at  the 
goal  position.  The  cost  associated  with  the  end¬ 
point  position  is 

Cgoal  = 


where 

Xd  =  Desired  Cartesian  tip  position 
Xe  =  Actual  Cartesian  tip  position 


It  is  essential  that  the  manipulator  avoid  any 
obstacles  that  may  be  present  in  its  workspace. 
To  do  this,  points  of  avoidance  are  specified 
along  the  links  of  the  manipulator.  The  cost 
associated  with  obstacle  avoidance  is 


I*..!  *,. 


where 

X^  =  Cartesian  position  of  the  ilh  point  of 
avoidance 

X0ij  =  Cartesian  Position  of  the  jlh  obstacle 
1  =  Number  of  points  of  avoidance 

m  =  Number  of  obstacles 

The  static  joint  torques  required  in  a  given 
configuration  are  a  function  of  the  external 
forces  applied  and  the  weights  of  each  link. 
Any  force  on  the  manipulator  can  be  mapped 
to  the  static  joint  torques  required  for  equilibri¬ 
um  using  the  Jacobian  of  the  manipulator  at  the 
point  where  the  force  is  applied.  This  mapping 
is  given  by 

xa=Jr(0)F 


J  =  Jacobian  of  the  manipulator 
F  =  Force  applied  to  manipulator 
Therefore,  the  static  joint  torques  re¬ 
quired  arc  given  by 


where 

Mj  =  Jacobian  associated  with  the 
center  of  mass  of  the  ilh  link 
Fj  =  Force  of  the  weight  of  the  i,h 
link 

Jj  =  Jacobian  associated  with  the 

location  of  the  j'h  point  load 
^  -  j'h  point  load 

N  =  Number  of  links 

n  -  Number  of  point  loads 

The  cost  associated  with  the  static 
torques  at  the  joints  is  given  by 


where 

T;  =  Torque  on  i'h  joint 

xm«i  ~  Maximum  torque  on  i,h  joint 

The  final  element  of  the  cost  function  is 
the  manipulability,  which  is  designed  to 
avoid  configurations  near  joint-space 
singularities  where  the  manipulator  is 
difficult  to  control.  The  cost  associated 
with  manipulability  is  given  by 

c  = - — - 

man  Det[JJT  ] 


The  complete  cost  function  can  be  for¬ 
mulated  as  a  weighted  sum  of  the  ele¬ 
ments  of  the  individual  cost  functions. 
This  is  given  by 


where 


=  Joint  torque  vector 


Cost=a  Cgoal  *  p  Cobs + y  Cman +eCcorQ 

As  Pin  [2J  has  pointed  out,  this  cost  function 
can  be  trapped  in  local  minima  when  using  a 
gradient  descent  algorithm.  Therefore,  the  EP 
algorithm  is  an  attractive  alternative. 

The  EP  Algorithm 

The  evolutionary  programming  algorithm  per¬ 
forms  a  search  by  methods  analogous  to  those 
found  in  natural  evolution.  The  algorithm 
involves  generating  an  initial  population,  mutat¬ 
ing  this  population,  determining  the  fitness  of 
each  element  of  the  population,  competing  ele¬ 
ments  of  the  population  against  each  other,  and 
selecting  the  most  fit  elements  of  the  popula¬ 
tion  to  survive  to  serve  as  the  parent  vectors  for 
the  next  generation. 

EP  can  be  described  by  the  following  seven-step 
procedure,  similar  to  the  one  found  in  Fogel 
[6], 

1.  Generate  an  initial  population  of  N 
configuration  vectors. 

2.  Mutate  each  of  the  N  parent  configura¬ 
tion  vectors  to  create  a  population  of  size 
2N. 

3.  Determine  the  cost  of  each  of  the  2N 
configuration  vectors. 


7.  Continue  with  Step  2. 

The  Mobile  Manipulator 

The  mobile  manipulator  used  in  these 
simulations  is  shown  in  Figure  1.  It  con¬ 
sists  of  a  platform  that  is  free  to  move  in 
one  dimension  with  a  three-link  planar 
manipulator  mounted  on  it.  The  link 
lengths  are  1.0,  1.0,  and  0.5  units,  respec¬ 
tively.  Each  link  has  a  density  of  10 
units  per  unit  length,  and  the  center  of 
gravity  is  at  the  midpoint  of  each  link. 
The  height  of  the  first  joint  of  the  ma¬ 
nipulator  is  0.5  units.  An  external  force 
of  0.5  units  is  applied  acting  downward 
at  the  tip  of  the  manipulator. 

The  size  of  the  mutation  for  each  ele¬ 
ment  of  the  population  is  a  function  of 
the  generation  number.  This  allows  the 
search  to  cover  a  large  range  of  possible 
configurations  and  still  to  refine  the 
configuration  vectors  as  the  search  pro¬ 
gresses.  The  size  of  the  mutations  is 
given  by 

6  =  pe~°-00lT* 

where 

6  =  Size  of  the  mutation 

p  =  N(0,1)  random  variable 

x  =  U(0,1)  random  variable 

k  =  Generation  number 


4.  Compete  each  member  of  the  population 
against  the  same  number  of  randomly- 
chosen  opponents,  assigning  a  win  to  the 
one  with  the  lowest  score. 

5.  Sort  the  population  by  decreasing  num¬ 
ber  of  wins. 

6.  Remove  the  N  configurations  with  the 
lowest  number  of  wins. 


The  goal  position  for  the  search  was 
located  at  the  Cartesian  coordinates 
(5,1.5),  as  designated  in  Figure  1  by  the 
filled  rectangle.  One  obstacle  was 
placed  at  the  Cartesian  coordinates 
(5,1.0),  as  designated  in  Figure  1  by  the 
circle.  One  point  of  avoidance  was 
chosen  at  the  midpoint  of  the  second 
link,  as  designated  in  Figure  1  by  the 
cross.  The  weights  in  the  cost  function 
were  chosen  to  be  100  on  Cgoal,  10  on 


Cobs,  1  on  CtonJ,  and  1  on  Cman  (a  =  100,  (1  = 
10,  y  =  1,  and  e  =  1). 

1'wo  different  approaches  were  compared.  The 
first  approach  used  a  configuration  vector 
consisting  of  the  cart  position  and  the  angle  of 
each  of  the  links  relative  to  the  horizontal, 
given  by 

Pi= 


This  representation  required  four  elements  in 
each  configuration  vector  and  relied  on  a  large 
weight  on  Cgoa,  to  force  the  end-point  to  the 
goal  position. 

The  second  approach  used  a  configuration 
vector  that  consists  of  only  the  angles  of  the 
outer  two  links  of  the  manipulator,  given  by 

Pi-  (02,03] 

The  cart  position  and  inner  joint  angle  v-crc 
calculated  so  that  the  end-point  was  at  the  goal 
position.  Therefore,  instead  of  requiring  four 
elements  in  the  configuration  vector,  only  two 
were  required. 

A  complication  of  employing  the  second  ap¬ 
proach  is  the  cart  position  and  first  angle  are 
not  uniquely  determined  by  the  outer  two 
angles,  but  can  take  on  two  distinct  sets  of 
values.  Tire  values  used  in  a  given  configu¬ 
ration  were  chosen  randomly  with  equal  proba¬ 
bility  to  allow  the  search  to  cover  as  large  a 
portion  of  the  space  as  possible. 

The  best  configuration  obtained  after  5000 
iterations  of  the  EP  algorithm  are  shown  in 
Figures  1  and  2  for  the  first  and  second  ap¬ 
proaches,  respectively.  'ITic  second  approach 
results  in  a  slightly  better  configuration  than  the 
first,  although  the  configurations  are  approxi¬ 
mately  the  same. 

However,  a  dramatic  difference  between  the 
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Figure  1: 

Converged  Solution 

when  Weighting  Tip  Position 


when  Constraining  Tip  Posi- 
t  ion 


two  approaches  can  be  seen  in  Figures  3 
and  4,  which  show  the  mean  and  be  A 
costs  for  the  two  approaches,  respec¬ 
tively.  The  difference  in  the  horizontal 
scales  • f  the  two  figures  should  be  not¬ 
ed.  Tire  first  approach  took  4875  itera¬ 
tions  to  converge  to  the  point  where  the 
value  of  the  cost  function  of  the  best 
configuration  was  within  0.01  of  its  final 
value.  The  second  approach,  however, 
took  only  15  iterations  to  reach  that 
point.  Clearly,  searching  for  the  optima 
using  an  equality  constraint  instead  of 
putting  a  large  penalty  on  the  cpd-point 
position  is  by  far  the  better  approach  for 
this  problem. 


Figure  3 :  Best  and  Mean  Cost  when 
Weighting  Tip  Position 


The  faster  convergence  using  the  second  ap¬ 
proach  was  not  the  result  of  starting  with  a 
better  set  of  parent  vectors  (vectors  which 
resulted  in  the  manipulator  end-point  being  at 
the  goal  position).  The  same  test  was  repeated 
for  the  unconstrained  search,  except  that  the 
initial  population  was  the  same  as  generated  for 
the  constrained  search.  The  final  configuration 
is  shown  in  Figure  5,  and  the  best  and  mean 
cost  for  this  search  are  shown  in  Figure  6. 
Although  the  search  converged  to  approximate¬ 
ly  the  same  final  configuration  as  the  other  two 
searches,  the  cost  is  slightly  higher.  It  took  over 
2200  iterations  before  this  search  found  a  better 
configuration  vector  than  the  best  one  from  its 
initial  population. 

There  are  two  possible  causes  for  this  dramatic 
improvement  in  convergence  speed  when  the 
end-point  position  is  constrained.  First,  the 
dimensionality  of  the  search  space  has  been 
reduced  by  constraining  the  end-point  position. 
Second,  the  use  of  a  large  weight  on  one  ele¬ 


ment  of  the  cost  function  while  the  other 
elements  have  much  smaller  weights 
results  in  a  search  space  that  is  difficult 
to  traverse.  The  effects  of  these  two 
factors  are  examined  in  the  next  two  sec¬ 
tions,  using  a  simplified  model. 

Large  Weights  in  the  Cost  Function 

Placing  a  large  weight  on  one  element  of 
the  cost  function  that  represents  a  par¬ 
ticularly  important  constraint  on  the 
system  can  result  a  in  search  space  that 
is  difficult  to  traverse.  In  this  section,  a 
simplified  model  is  presented  to  study 
the  effect  this  has  on  convergence. 

The  cost  function  chosen  to  study  this 
effect  was 

Cost- A  (x+y- 10)  2  +  (x-y) 2 


The  two  terms  of  this  cost  function  are 
minimized  along  different  lines  in  the 
Space  of  x  and  y.  The  minima  of  this 
function  occurs  where  x  =  5  and  y  =  5. 
There  are  no  local  minima  in  the  search 
space. 

The  search  space  for  this  cost  function  is 
shown  in  Figure  7  for  the  case  when 
A=100.  With  the  large  weight  on  the 
first  term  in  the  cost  function,  the  search 
surface  is  a  narrow,  steep  valley,  where 
the  floor  of  the  valley  is  nearly  flat  com¬ 
pared  to  the  steepness  of  the  walls. 
Once  an  evolutionary  search  has  found 
its  way  to  the  floor  of  the  valley,  it 
would  have  to  mutate  the  solution  in 
precisely  the  correct  direction  to  find  a 
better  solution  than  the  one  it  has  al¬ 
ready  found. 


To  illustrate  this,  let  a  parent  vector  be 
x  =  6  and  y  =  4,  and  let  the  mutation  of 


Figure  4:  Best  and  Mean  Cost  when 
Constraining  Tip  Position 


the  y  component  be  +0.1.  If  the  weight  A  on 
the  first  term  of  the  cost  function  is  100,  the 
mutated  x  component  must  fall  between  5.97 
and  5.79  to  result  in  a  better  solution,  a  range 
of  mutations  of  0.18.  If,  however,  the  weight  A 
is  10000,  the  mutated  x  component  must  be  be¬ 
tween  5.91  and  5.89  to  result  in  a  better  solu¬ 
tion,  a  range  of  only  0.02.  This  example  indi¬ 
cates  that  as  the  ratio  of  the  cost  function 
weights  increases,  the  probability  of  a  mutation 
being  better  than  its  parent  decreases  once  the 
best  parent  vector  has  reached  the  valley  floor. 


Figure  5:  Best  Configuration  when 
Weighting  Tip  Position  and  a  Start¬ 
ing  Population  with  Tip  at  Goal 
Position 


However,  if  the  solution  is  assumed  to 
satisfy  the  equality  constraint, 

1 0  =x+y 

the  search  surface  is  reduced  to  the 
curve  shown  in  Figure  8.  This  demon¬ 
strates  the  simplification  to  the  search 
surface  that  results  from  using  the  equal¬ 
ity  constraint. 

To  study  the  effect  of  the  ratio  of  the 
weights,  the  value  of  A  was  varied.  For 
each  value  of  A,  ten  evolutionary  search¬ 
es  were  conducted,  each  starting  with  a 
different  parent  population.  Each 
search  was  continued  until  the  best  solu¬ 
tion  was  within  a  radius  of  0.01  units  of 
the  actual  minima  of  the  function.  Fig¬ 
ure  9  shows  the  average  of  the  number 
of  iterations  to  achieve  this  level  of 
convergence  as  a  function  of  the  value 
of  A.  This  indicates  that  the  number  of 
iterations  required  for  convergence 
increases  as  the  ratio  of  the  weights  (the 
steepness  of  the  valley  walls)  increases. 
However,  the  number  of  iterations  in¬ 
creases  by  only  approximately  a  factor  of 
seven  as  the  weight  ratio  increases  by 
seven  orders  of  magnitude.  For  this 
system,  the  effect  of  the  weight  ratio 
does  not  increase  dramatically  until  very 
large  weight  ratios  are  employed. 

The  difficulties  of  searching  in  these  nar¬ 
row,  steep  valleys  with  relatively  flat 
floors  might  be  overcome  by  augmenting 
the  evolutionary  search  with  a  local 
search  method.  This  approach  has  been 
suggested  by  Waagen  et  al  [7],  using  the 
direction  set  algorithm  as  the  local  met¬ 
hod. 

Number  of  Parameters  in  Search 
The  other  factor  that  increases  the  effi- 


Figure  6:  Best  and  Mean  Cost  when 
Weighting  Tip  Position  and  a  Start¬ 
ing  Population  with  Tip  at  Goal 
Position 

ciency  of  the  search  when  using  an  equality 
constraint  is  the  reduction  in  the  dimensionality 
of  the  search.  Instead  of  searching  for  four  pa¬ 
rameters,  the  search  is  attempting  to  find  the 
minima  for  two  parameters.  In  this  section,  the 
effect  of  reducing  the  number  of  parameters  is 
examined. 


Figure  7:  Search  Space  of  Simpli¬ 
fied  Model 


To  study  the  effect  of  the  number  of 
parameters,  the  simplified  model  was 
modified.  The  cost  function  was  modi¬ 
fied  to  be  a  function  of  twelve  variables, 
Xj  through  x*,  and  y1  through  y6.  The 
cost  function  was  specified  by  the  three 
equations 


Cost= 1000 (x+y-10) (x-y) 2 


+ 


The  minima  of  this  cost  function  occurs 
when  all  the  x,  and  y,  are  5/6. 

First,  the  minima  was  found  searching 
for  all  twelve  variables.  The  search  was 
repeated  ten  times,  starting  from  differ¬ 
ent  randomly-selected  parent  vectors, 
and  the  average  number  of  iterations  to 
converge  to  a  solution  within  0.01  units 
of  the  actual  solution  was  determined. 
This  process  was  repeated  five  additional 
times,  each  time  constraining  an  addi¬ 
tional  pair  of  ^  and  yj  to  the  values 
where  the  function  was  minimized. 
Figure  10  shows  the  average  number  of 
iterations  for  convergence  as  a  function 
of  the  number  of  parameters  which  the 
search  attempted  to  find.  This  number 
of  iterations  does  not  increase  monoto- 
nically  with  the  number  of  parameters  in 
the  search  as  was  expected.  While  the 
general  trend  is  upward,  it  is  not  clear 
why  the  number  of  iterations  drop  for 
increasing  number  of  parameters. 

It  is  interesting  to  note  that  the  com¬ 
bined  effect  of  the  reduction  in  dimen¬ 
sionality  and  the  simplification  of  the 
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search  surface  for  this  simplified  model  can 
only  account  for  a  reduction  on  the  order  of  ten 
times,  whereas  the  increase  in  speed  of  conver¬ 
gence  using  the  equality  constraint  for  the 
mobile  manipulator  was  on  the  order  of  one 
hundred  times.  This  discrepancy  might  be 
explained  by  the  greater  complexity  of  the 
search  surface  for  the  mobile  manipulator. 

Conclusions 

Evolutionary  programming  has  been  applied  to 
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the  problem  of  finding  the  best  configu¬ 
ration  for  a  mobile  manipulator  with 
redundant  degrees  of  freedom.  'Die 
search  was  conducted  both  by  placing  a 
large  penalty  on  the  deviations  of  the 
end-point  position  from  the  goal  position 
and  by  fixing  the  end-point  at  the  goal 
position  and  only  searching  for  two 
elements  of  the  configuration  vector. 
The  search  converged  far  faster  when 
the  end-point  of  the  manipulator  was 
constrained. 

This  increase  in  performance  is  due  to 
two  factors:  the  search  surface  is  difficult 
to  traverse  when  one  factor  of  the  cost 
function  is  weighted  significantly  more 
than  the  other;  and  the  dimensionality  of 
the  search  has  been  reduced.  The  re¬ 
sults  of  an  analysis  of  a  simplified  model 
indicate  that  these  factors  increase  the 
convergence  speed  by  approximately  one 
order  of  magnitude,  but  the  combined 
effect  does  not  seem  to  actually  account 
for  the  total  effect  seen  in  the  simulation 
of  the  actual  manipulator. 

In  a  practical  implementation  of  configu¬ 
ration  optimization  using  EP,  using  the 
end-point  constraint  yields  superior 
performance.  For  a  general  system,  the 
dimensionality  of  the  search  can  only  be 
reduced  if  there  is  some  a  priori  knowl¬ 
edge  of  the  minima  of  the  system.  If 
possible,  one  should  incorporate  the 
constraints  to  speed  convergence. 
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